package com.lego.jbv;

import lejos.nxt.*;
import lejos.util.Delay;
/*
 * Follow behavior , inspired by p. 304 in
 * Jones, Flynn, and Seiger: 
 * "Mobile Robots, Inspiration to Implementation", 
 * Second Edition, 1999.
 */

class Follow extends Thread
{
    private SharedCar car = new SharedCar();

	private int power = 70, ms = 500;
	LightSensor light = new LightSensor(SensorPort.S3);
	private NXTRegulatedMotor scannerMotor;
	private int scannerAngle = 45;
	
	int frontLight, leftLight, rightLight, delta;
	int lightThreshold;
	
    public Follow(SharedCar car)
    {
       this.car = car;	
       lightThreshold = light.getLightValue();
       scannerMotor = new NXTRegulatedMotor(MotorPort.A);
       scannerMotor.setSpeed(360);
    }
    
	public void run() 
    {				       
        while (true)
        {
	    	// Monitor the light in front of the car and start to follow
	    	// the light if light level is above the threshold
        	frontLight = light.getLightValue();
	    	while ( frontLight <= lightThreshold )
	    	{
	    		car.noCommand();
	    		frontLight = light.getLightValue();
	    	}
	    	
	    	// Follow light as long as the light level is above the threshold
	    	while ( frontLight > lightThreshold )
	    	{
	    		scannerMotor.rotateTo(scannerAngle);
	    		leftLight = light.getLightValue();
	    		
	    		
	    		scannerMotor.rotateTo(-scannerAngle);
	    		rightLight = light.getLightValue();
	    		
	    		scannerMotor.rotateTo(0, true);
	    		
	    		// Follow light for a while
	    		delta = leftLight-rightLight;
	    		car.forward(power+delta, power-delta);
	    		Delay.msDelay(ms);
    		
	    		frontLight = light.getLightValue();
	    	}
	    	
	    	car.stop();
	    	Delay.msDelay(ms);
 			
        }
    }
}

/*
scannerAngle = angle;
    	scannerMotor = motor;
    	
    	if(scannerIniPos == 1000)
    		scannerIniPos = scannerMotor.getPosition();
    	else
    	{
    		scannerMotor.setSpeed(360);
        	scannerMotor.rotateTo(scannerIniPos);
    	}
    	
    	scannerMotor.waitComplete();
    	scannerMotor.setSpeed(degreesPrSecond);
    	
    	Thread scannerThread = new Thread(new Runnable() {
			@Override
			public void run() {
				char state = 0;
				isScanning = true;
				
				while(isScanning)
				{	
					if(++state%2 > 0)
						scannerMotor.rotateTo(scannerAngle);
					else
						scannerMotor.rotateTo(-scannerAngle);
				}
			}
		});
*/